/*
 * mod_math.c
 *
 *  Created on: 2025 Feb 17
 *      Author: SFLY
 */
#include "mod_math.h"
#include "driverlib.h"
#define Q10_BASE    0x00000400L
#define Q10(X)      ((int32_t)((Q10_BASE)*(X)))

// 归一化角度到 [0,2PI]
float normalizeAngle(float angle){
  float a = fmodf(angle, MATH_TWO_PI);   //取余运算可以用于归一化，列出特殊值例子算便知
  return a >= 0 ? a : (a + MATH_TWO_PI);
  //三目运算符。格式：condition ? expr1 : expr2
  //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。
  //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。
  //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。
}



/******************************************************************************/
/* Global variable definitions (declared in header file with ('extern')       */
/******************************************************************************/
const float f32SinTbl[257] = {                                                            0.0f,     /** 0 */
0.00614f,   0.01227f,   0.01841f,   0.02454f,   0.03067f,   0.03681f,   0.04294f,   0.04907f,    0.0552f,   0.06132f,    /** 10 */
0.06744f,   0.07356f,   0.07968f,    0.0858f,   0.09191f,   0.09802f,   0.10412f,   0.11022f,   0.11632f,   0.12241f,    /** 20 */
 0.1285f,   0.13458f,   0.14066f,   0.14673f,    0.1528f,   0.15886f,   0.16491f,   0.17096f,     0.177f,   0.18304f,    /** 30 */
0.18907f,   0.19509f,    0.2011f,   0.20711f,   0.21311f,    0.2191f,   0.22508f,   0.23106f,   0.23702f,   0.24298f,    /** 40 */
0.24893f,   0.25487f,   0.26079f,   0.26671f,   0.27262f,   0.27852f,   0.28441f,   0.29028f,   0.29615f,   0.30201f,    /** 50 */
0.30785f,   0.31368f,    0.3195f,   0.32531f,   0.33111f,   0.33689f,   0.34266f,   0.34842f,   0.35416f,    0.3599f,    /** 60 */
0.36561f,   0.37132f,   0.37701f,   0.38268f,   0.38835f,   0.39399f,   0.39962f,   0.40524f,   0.41084f,   0.41643f,    /** 70 */
  0.422f,   0.42756f,   0.43309f,   0.43862f,   0.44412f,   0.44961f,   0.45508f,   0.46054f,   0.46598f,    0.4714f,    /** 80 */
 0.4768f,   0.48218f,   0.48755f,    0.4929f,   0.49823f,   0.50354f,   0.50883f,    0.5141f,   0.51936f,   0.52459f,    /** 90 */
 0.5298f,     0.535f,   0.54017f,   0.54532f,   0.55046f,   0.55557f,   0.56066f,   0.56573f,   0.57078f,   0.57581f,    /** 100 */
0.58081f,    0.5858f,   0.59076f,    0.5957f,   0.60062f,   0.60551f,   0.61038f,   0.61523f,   0.62006f,   0.62486f,    /** 110 */
0.62964f,   0.63439f,   0.63912f,   0.64383f,   0.64851f,   0.65317f,   0.65781f,   0.66242f,     0.667f,   0.67156f,    /** 120 */
0.67609f,    0.6806f,   0.68508f,   0.68954f,   0.69397f,   0.69838f,   0.70275f,   0.70711f,   0.71143f,   0.71573f,    /** 130 */
   0.72f,   0.72425f,   0.72846f,   0.73265f,   0.73682f,   0.74095f,   0.74506f,   0.74914f,   0.75319f,   0.75721f,    /** 140 */
 0.7612f,   0.76517f,    0.7691f,   0.77301f,   0.77689f,   0.78074f,   0.78456f,   0.78835f,   0.79211f,   0.79584f,    /** 150 */
0.79954f,   0.80321f,   0.80685f,   0.81046f,   0.81404f,   0.81758f,    0.8211f,   0.82459f,   0.82805f,   0.83147f,    /** 160 */
0.83486f,   0.83822f,   0.84155f,   0.84485f,   0.84812f,   0.85136f,   0.85456f,   0.85773f,   0.86087f,   0.86397f,    /** 170 */
0.86705f,   0.87009f,   0.87309f,   0.87607f,   0.87901f,   0.88192f,    0.8848f,   0.88764f,   0.89045f,   0.89322f,    /** 180 */
0.89597f,   0.89867f,   0.90135f,   0.90399f,    0.9066f,   0.90917f,   0.91171f,   0.91421f,   0.91668f,   0.91911f,    /** 190 */
0.92151f,   0.92388f,   0.92621f,   0.92851f,   0.93077f,   0.93299f,   0.93518f,   0.93734f,   0.93946f,   0.94154f,    /** 200 */
0.94359f,   0.94561f,   0.94759f,   0.94953f,   0.95144f,   0.95331f,   0.95514f,   0.95694f,    0.9587f,   0.96043f,    /** 210 */
0.96212f,   0.96378f,   0.96539f,   0.96698f,   0.96852f,   0.97003f,    0.9715f,   0.97294f,   0.97434f,    0.9757f,    /** 220 */
0.97703f,   0.97832f,   0.97957f,   0.98079f,   0.98196f,   0.98311f,   0.98421f,   0.98528f,   0.98631f,    0.9873f,    /** 230 */
0.98826f,   0.98918f,   0.99006f,    0.9909f,   0.99171f,   0.99248f,   0.99321f,   0.99391f,   0.99456f,   0.99518f,    /** 240 */
0.99577f,   0.99631f,   0.99682f,   0.99729f,   0.99772f,   0.99812f,   0.99848f,    0.9988f,   0.99908f,   0.99932f,    /** 250 */
0.99953f,    0.9997f,   0.99983f,   0.99992f,   0.99998f,       1.0f,                                                              /** 256 */
};

/******************************************************************************/
/* Function implementation - global ('extern') and local ('static')           */
/******************************************************************************/

/**
 ******************************************************************************
 ** \brief sine function by look-up table
 **
 ** \param [in] f32angle: Q24 format P.U. angle
 **
 ** \retval     f32-sin(angle)
 ******************************************************************************/
float __attribute__((optnone)) Math_Sin(float f32angle)
{
    float f32sin;
    int32_t i32Q10_angle = Q10(f32angle);
    i32Q10_angle &= 0x3FF;

    if (i32Q10_angle <= 512) {
        if (i32Q10_angle <= 256) {
            f32sin = f32SinTbl[i32Q10_angle];
        } else {
            i32Q10_angle = 512 - i32Q10_angle;
            f32sin = f32SinTbl[i32Q10_angle];
        }
    } else {
        if (i32Q10_angle <= 768) {
            i32Q10_angle -= 512;
            f32sin = - f32SinTbl[i32Q10_angle];
        } else {
            i32Q10_angle = 1024 - i32Q10_angle;
            f32sin = -f32SinTbl[i32Q10_angle];
        }
    }
    return (f32sin);
}

/**
 ******************************************************************************
 ** \brief cosine function by look-up table
 **
 ** \param [in] f32angle: Q24 format P.U. angle
 **
 ** \retval     f32-cos(angle)
 ******************************************************************************/
float __attribute__((optnone)) Math_Cos(float f32angle)
{
    return (Math_Sin(f32angle + 0.25f));
}
/**
 ******************************************************************************
 ** \brief sine/cosine function by look-up table
 **
 ** \param [in] i32Q24_angle: Q24 format P.U. angle
 **
 ** \retval     Q15-sin(angle), Q15-cos(angle)
 ******************************************************************************/
void __attribute__((optnone)) Math_SinCos(float f32angle, float *f32sin, float *f32cos)
{
    int32_t i32Q10_angle = Q10(f32angle);
    i32Q10_angle &= 0x3FF;

    if (i32Q10_angle <= 512) {
        if (i32Q10_angle <= 256) {
            *f32sin = f32SinTbl[i32Q10_angle];
            *f32cos = f32SinTbl[256 - i32Q10_angle];
        } else {
            *f32sin = f32SinTbl[512 - i32Q10_angle];
            *f32cos = -f32SinTbl[i32Q10_angle - 256];
        }
    } else {
        if (i32Q10_angle <= 768) {
            *f32sin = -f32SinTbl[i32Q10_angle - 512];
            *f32cos = -f32SinTbl[768 - i32Q10_angle];
        } else {
            *f32sin = -f32SinTbl[1024 - i32Q10_angle];
            *f32cos = f32SinTbl[i32Q10_angle - 768];
        }
    }
}

/**
 ******************************************************************************
 ** \brief Math_Atan2(x, y) function
 **         1. the input x, and y should have the same Q-format
 **         2. both x and y should less than 2^24, otherwise precision degrades
 **
 ** \param [in] f32x: x-axis value, < 2^24
 ** \param [in] f32y: y-axis value, < 2^24
 **
 ** \retval     float32_t f32angle: Q24 format P.U. angle
 ******************************************************************************/
float __attribute__((optnone)) Math_Atan2(float f32x, float f32y)
{
    float f32angle;
    float f32Tan;

    float f32XX = f32x;
    float f32YY = f32y;

    float f32Absx,f32Absy,f32AbsTan;

        if(f32x >= 0.0f)
        {
                f32Absx = f32x;
        }
        else
        {
                f32Absx = -f32x;
        }

        if(f32y >= 0.0f)
        {
                f32Absy = f32y;
        }
        else
        {
                f32Absy = -f32y;
        }

    /** special case handling *************************************************/
    if(0.0f == f32XX)
    {
        if (f32YY > 0.0f)
        {
            f32angle = 0.25f;        // 90 degree
        }
        else if (f32YY < 0.0f)
        {
            f32angle = 0.75f;        // 270 degree
        }
        else
        {
            f32angle = 0.0f;           // 0 degree
        }

        return f32angle;
    }
    if(0.0f == f32YY)
    {
        if (f32XX > 0.0f)
        {
            f32angle = 0.0f;           // 0 degree
        }
        else
        {
            f32angle = 0.5f;     // 180 degree
        }

        return f32angle;
    }

    /** atan(a) = a/8 + [0.2731*a* (1-|a|)]/(2*pi), in PU, with -1<a<1 ********/
    if (f32Absx >= f32Absy)
    {
                f32Tan = f32YY / f32XX;
                //f32Tan = __builtin_dsp_fsdiv_qx320f(f32YY,f32XX);
                if(f32Tan >= 0.0f)
                {
                        f32AbsTan = f32Tan;
                }
                else
                {
                        f32AbsTan = -f32Tan;
                }
        f32angle = f32Tan * 0.125f; // a/8
        f32Tan = f32Tan * (1.0f - f32AbsTan); //remain term, <0.25
        f32angle += f32Tan * 0.043465f;  // 0.043465 = 0.2731/2/pi

        if (f32x > 0.0f)
        {
                        if(f32angle > 1.0f)
                        {
                                f32angle -= 1.0f;
                        }
                        else if(f32angle < 0.0f)
                        {
                                f32angle += 1.0f;
                        }
                        else
                        {

                        }
        }
        else
        {
            f32angle += 0.5f;
        }
    }
    else
    {
        f32Tan = f32XX / f32YY;
        //f32Tan = __builtin_dsp_fsdiv_qx320f(f32XX,f32YY);
                if(f32Tan >= 0.0f)
                {
                        f32AbsTan = f32Tan;
                }
                else
                {
                        f32AbsTan = -f32Tan;
                }
                f32angle = f32Tan * 0.125f; // a/8
        f32Tan = f32Tan * (1.0f - f32AbsTan);  //remain term, <0.25
        f32angle += f32Tan * 0.043465f;  // 0.043465 = 0.2731/2/pi

        if (f32y > 0.0f)
        {
            f32angle = 0.25f - f32angle;
        }
        else
        {
            f32angle = 0.75f - f32angle;
        }
    }

    return f32angle;
}


// 将指定位置为 1
uint32_t setBit(uint32_t num, int position) {
    // 创建掩码，将对应位置设为 1
    uint32_t mask = 1U << position;
    // 按位或操作，将指定位置为 1
    return num | mask;
}



